Imports System.Collections

''' <summary>
''' This is a game component that implements IUpdateable.
''' </summary>
Public Class Collision
    Inherits Microsoft.Xna.Framework.GameComponent

    Public Structure Intersection
        Public Distance As Single
        Public VertexIndex As Integer
    End Structure

    Public Sub New(ByVal game As DanoQuest)
        MyBase.New(game)
        game.Components.Add(Me)
        ' TODO: Construct any child components here
    End Sub

    Public Function RayIntersectsTriangle(ByRef ray As Ray, ByRef vertex1 As Vector3, ByRef vertex2 As Vector3, ByRef vertex3 As Vector3) As System.Nullable(Of Single)
        Dim edge1 As Vector3, edge2 As Vector3
        Vector3.Subtract(vertex2, vertex1, edge1)
        Vector3.Subtract(vertex3, vertex1, edge2)
        Dim directionCrossEdge2 As Vector3
        Vector3.Cross(ray.Direction, edge2, directionCrossEdge2)
        Dim determinant As Single
        Vector3.Dot(edge1, directionCrossEdge2, determinant)
        If (determinant > -Single.Epsilon) And (determinant < Single.Epsilon) Then
            Return Nothing
        End If
        Dim inverseDeterminant As Single = 1.0F / determinant
        Dim distanceVector As Vector3
        Vector3.Subtract(ray.Position, vertex1, distanceVector)
        Dim triangleU As Single
        Vector3.Dot(distanceVector, directionCrossEdge2, triangleU)
        triangleU = triangleU * inverseDeterminant
        If triangleU < 0 Or triangleU > 1 Then
            Return Nothing
        End If
        Dim distanceCrossEdge1 As Vector3
        Vector3.Cross(distanceVector, edge1, distanceCrossEdge1)
        Dim triangleV As Single
        Vector3.Dot(ray.Direction, distanceCrossEdge1, triangleV)
        triangleV = triangleV * inverseDeterminant
        If triangleV < 0 Or (triangleU + triangleV) > 1 Then
            Return Nothing
        End If
        Dim rayDistance As Single
        Vector3.Dot(edge2, distanceCrossEdge1, rayDistance)
        rayDistance = rayDistance * inverseDeterminant
        If rayDistance < 0 Then
            Return Nothing
        End If
        Return rayDistance
    End Function

    Public Function Distance(ByVal Ray As Ray, ByVal Model As Model, ByVal ModelWorldTransforms As Matrix, Optional ByRef Angle As System.Nullable(Of Integer) = Nothing, Optional ByRef pickedtriangle As VertexPositionColor() = Nothing) As Single
        Dim ClosestIntersection As New Intersection With {.Distance = Single.MaxValue, .VertexIndex = 0}
        Dim InverseTransform As Matrix = Matrix.Invert(ModelWorldTransforms)
        Ray.Position = Vector3.Transform(Ray.Position, InverseTransform)
        Ray.Direction = Vector3.TransformNormal(Ray.Direction, InverseTransform)
        Dim tagData As Dictionary(Of String, Object) = DirectCast(Model.Tag, Dictionary(Of String, Object))
        If tagData Is Nothing Then
            Throw New InvalidOperationException("Model.Tag is not set correctly. Make sure your model was built using the custom TrianglePickingProcessor.")
        End If
        If DirectCast(tagData("BoundingSphere"), BoundingSphere).Intersects(Ray) Is Nothing Then
            Return Nothing
        Else
            Dim Vertices As Vector3() = DirectCast(tagData("Vertices"), Vector3())
            Dim intersection As System.Nullable(Of Single)
            For i As Integer = 0 To Vertices.Length - 1 Step 3
                intersection = RayIntersectsTriangle(Ray, Vertices(i), Vertices(i + 1), Vertices(i + 2))
                If intersection IsNot Nothing Then
                    If intersection < ClosestIntersection.Distance Then
                        ClosestIntersection.Distance = intersection.Value 'Todo: We can skip this if they dont want a triangle
                        ClosestIntersection.VertexIndex = i
                    End If
                End If
            Next
            If Angle IsNot Nothing Or pickedtriangle IsNot Nothing Then 'We could add code here to update pickedtriangle in threedelements on triangle change...
                ReDim pickedtriangle(3)
                pickedtriangle = {New VertexPositionColor(Vector3.Zero, Color.Magenta), New VertexPositionColor(Vector3.Zero, Color.Magenta), New VertexPositionColor(Vector3.Zero, Color.Magenta)}
                Vector3.Transform(Vertices(ClosestIntersection.VertexIndex), ModelWorldTransforms, pickedtriangle(0).Position)
                Vector3.Transform(Vertices(ClosestIntersection.VertexIndex + 1), ModelWorldTransforms, pickedtriangle(1).Position)
                Vector3.Transform(Vertices(ClosestIntersection.VertexIndex + 2), ModelWorldTransforms, pickedtriangle(2).Position)
                If Angle IsNot Nothing Then 'Dont bother calculating the slope if its not asked for
                    Dim TriangleSlope As Vector3 = Vector3.Cross(pickedtriangle(1).Position - pickedtriangle(0).Position, pickedtriangle(2).Position - pickedtriangle(0).Position)
                    If TriangleSlope = Vector3.Zero Then 'We hit a vertical wall
                        Angle = 180
                    Else
                        Angle = CInt(MathHelper.ToDegrees(CSng(Math.Acos(Vector3.Dot(Vector3.Normalize(TriangleSlope), Vector3.Down)))))
                    End If
                End If
            End If
        End If
        Return ClosestIntersection.Distance
    End Function

    ''' <summary>
    ''' Allows the game component to perform any initialization it needs to before starting
    ''' to run.  This is where it can query for any required services and load content.
    ''' </summary>
    Public Overrides Sub Initialize()
        ' TODO: Add your initialization code here
        MyBase.Initialize()
    End Sub

    ''' <summary>
    ''' Allows the game component to update itself.
    ''' </summary>
    ''' <param name="gameTime">Provides a snapshot of timing values.</param>
    Public Overrides Sub Update(ByVal gameTime As GameTime)
        ' TODO: Add your update code here
        MyBase.Update(gameTime)
    End Sub

End Class



'Public Function Distance(ByVal Ray As Ray, ByVal Model As Model, ByVal ModelWorldTransforms As System.Nullable(Of Matrix)) As String
'    Try
'        Dim Vertex1 As Vector3, Vertex2 As Vector3, Vertex3 As Vector3
'        Dim ClosestIntersection As System.Nullable(Of Single) = Nothing
'        Dim InverseTransform As Matrix = Matrix.Invert(ModelWorldTransforms)
'        Ray.Position = Vector3.Transform(Ray.Position, InverseTransform)
'        Ray.Direction = Vector3.TransformNormal(Ray.Direction, InverseTransform)
'        Dim tagData As Dictionary(Of String, Object) = DirectCast(Model.Tag, Dictionary(Of String, Object))
'        If tagData Is Nothing Then
'            Throw New InvalidOperationException("Model.Tag is not set correctly. Make sure your model was built using the custom TrianglePickingProcessor.")
'        End If
'        If DirectCast(tagData("BoundingSphere"), BoundingSphere).Intersects(Ray) Is Nothing Then
'            Return Nothing
'        Else
'            Dim Vertices As Vector3() = DirectCast(tagData("Vertices"), Vector3())
'            For i As Integer = 0 To Vertices.Length - 1 Step 3
'                Dim intersection As System.Nullable(Of Single)
'                Dim edge1 As Vector3, edge2 As Vector3
'                Vector3.Subtract(Vertices(i + 1), Vertices(i), edge1)
'                Vector3.Subtract(Vertices(i + 2), Vertices(i), edge2)
'                Dim directionCrossEdge2 As Vector3
'                Vector3.Cross(Ray.Direction, edge2, directionCrossEdge2)
'                Dim determinant As Single
'                Vector3.Dot(edge1, directionCrossEdge2, determinant)
'                If (determinant > -Single.Epsilon) And (determinant < Single.Epsilon) Then
'                    intersection = Nothing
'                End If
'                Dim inverseDeterminant As Single = 1.0F / determinant
'                Dim distanceVector As Vector3
'                Vector3.Subtract(Ray.Position, Vertices(i), distanceVector)
'                Dim triangleU As Single
'                Vector3.Dot(distanceVector, directionCrossEdge2, triangleU)
'                triangleU = triangleU * inverseDeterminant
'                If triangleU < 0 Or triangleU > 1 Then
'                    intersection = Nothing
'                End If
'                Dim distanceCrossEdge1 As Vector3
'                Vector3.Cross(distanceVector, edge1, distanceCrossEdge1)
'                Dim triangleV As Single
'                Vector3.Dot(Ray.Direction, distanceCrossEdge1, triangleV)
'                triangleV = triangleV * inverseDeterminant
'                If triangleV < 0 Or (triangleU + triangleV) > 1 Then
'                    intersection = Nothing

'                End If
'                Dim rayDistance As Single
'                Vector3.Dot(edge2, distanceCrossEdge1, rayDistance)
'                rayDistance = rayDistance * inverseDeterminant
'                If rayDistance < 0 Then
'                    intersection = Nothing

'                Else
'                    intersection = rayDistance
'                End If
'                If intersection IsNot Nothing Then
'                    If (ClosestIntersection Is Nothing) Or (intersection < ClosestIntersection) Then
'                        ClosestIntersection = intersection.Value
'                        Vector3.Transform(Vertices(i), ModelWorldTransforms, Vertex1)
'                        Vector3.Transform(Vertices(i + 1), ModelWorldTransforms, Vertex2)
'                        Vector3.Transform(Vertices(i + 2), ModelWorldTransforms, Vertex3)
'                    End If
'                End If
'            Next
'        End If
'        Return ClosestIntersection.ToString 'If you use .value it returns an error, so we use a string instead for now
'    Catch exception As System.Exception
'        Return exception.Message.ToString
'    Finally
'    End Try
'End Function

'=================================================================================================


'Public Function RayIntersectsModel(ray As Ray, model As Model, modelTransform As Matrix, ByRef vertex1 As Vector3, ByRef vertex2 As Vector3, ByRef vertex3 As Vector3) As System.Nullable(Of Single)
'    Dim inverseTransform As Matrix = Matrix.Invert(modelTransform)
'    ray.Position = Vector3.Transform(ray.Position, inverseTransform)
'    ray.Direction = Vector3.TransformNormal(ray.Direction, inverseTransform)
'    Dim tagData As Dictionary(Of String, Object) = DirectCast(model.Tag, Dictionary(Of String, Object))
'    If tagData Is Nothing Then
'        Throw New InvalidOperationException("Model.Tag is not set correctly. Make sure your model was built using the custom TrianglePickingProcessor.")
'    End If
'    Dim boundingSphere As BoundingSphere = DirectCast(tagData("BoundingSphere"), BoundingSphere)
'    If boundingSphere.Intersects(ray) Is Nothing Then
'        Return Nothing
'    Else
'        Dim closestIntersection As System.Nullable(Of Single) = Nothing
'        Dim vertices As Vector3() = DirectCast(tagData("Vertices"), Vector3())
'        For i As Integer = 0 To vertices.Length - 1 Step 3
'            Dim intersection As System.Nullable(Of Single)
'            RayIntersectsTriangle(ray, vertices(i), vertices(i + 1), vertices(i + 2), intersection)
'            If intersection IsNot Nothing Then
'                If (closestIntersection Is Nothing) Or (intersection < closestIntersection) Then
'                    closestIntersection = intersection
'                    Vector3.Transform(vertices(i), modelTransform, vertex1)
'                    Vector3.Transform(vertices(i + 1), modelTransform, vertex2)
'                    Vector3.Transform(vertices(i + 2), modelTransform, vertex3)
'                End If
'            End If
'        Next
'        Return closestIntersection
'    End If
'End Function

'Public Sub RayIntersectsTriangle(ByRef ray As Ray, ByRef vertex1 As Vector3, ByRef vertex2 As Vector3, ByRef vertex3 As Vector3, ByRef result As System.Nullable(Of Single))
'    Dim edge1 As Vector3, edge2 As Vector3
'    Vector3.Subtract(vertex2, vertex1, edge1)
'    Vector3.Subtract(vertex3, vertex1, edge2)
'    Dim directionCrossEdge2 As Vector3
'    Vector3.Cross(ray.Direction, edge2, directionCrossEdge2)
'    Dim determinant As Single
'    Vector3.Dot(edge1, directionCrossEdge2, determinant)
'    If (determinant > -Single.Epsilon) And (determinant < Single.Epsilon) Then
'        result = Nothing
'        Return
'    End If
'    Dim inverseDeterminant As Single = 1.0F / determinant
'    Dim distanceVector As Vector3
'    Vector3.Subtract(ray.Position, vertex1, distanceVector)
'    Dim triangleU As Single
'    Vector3.Dot(distanceVector, directionCrossEdge2, triangleU)
'    triangleU = triangleU * inverseDeterminant
'    If triangleU < 0 Or triangleU > 1 Then
'        result = Nothing
'        Return
'    End If
'    Dim distanceCrossEdge1 As Vector3
'    Vector3.Cross(distanceVector, edge1, distanceCrossEdge1)
'    Dim triangleV As Single
'    Vector3.Dot(ray.Direction, distanceCrossEdge1, triangleV)
'    triangleV = triangleV * inverseDeterminant
'    If triangleV < 0 Or (triangleU + triangleV) > 1 Then
'        result = Nothing
'        Return
'    End If
'    Dim rayDistance As Single
'    Vector3.Dot(edge2, distanceCrossEdge1, rayDistance)
'    rayDistance = rayDistance * inverseDeterminant
'    '  test = rayDistance.ToString
'    If rayDistance < 0 Then
'        result = Nothing
'        Return
'    End If
'    result = rayDistance
'End Sub

'Public Function distance(ByVal model As Model, ByVal modelWorldTransforms As Matrix) As Single
'    Dim cursorRay As Ray = New Ray(Game.Components.OfType(Of Camera).ToList.Item(0).CameraPosition, New Vector3(0, -1, 0)) 'Collision checks with camera and top of mesh 
'    Dim pickedModelName As String = Nothing
'    Dim closestIntersection As Single = Single.MaxValue
'    Dim vertex1 As Vector3, vertex2 As Vector3, vertex3 As Vector3
'    Dim intersection As System.Nullable(Of Single) = RayIntersectsModel(cursorRay, model, modelWorldTransforms, vertex1, vertex2, vertex3)
'    Dim pickedTriangle As VertexPositionColor() = {New VertexPositionColor(Vector3.Zero, Color.Magenta), New VertexPositionColor(Vector3.Zero, Color.Magenta), New VertexPositionColor(Vector3.Zero, Color.Magenta)}

'    If intersection IsNot Nothing Then
'        If intersection < closestIntersection Then
'            closestIntersection = intersection.Value
'            pickedModelName = "Terrain"
'            pickedTriangle(0).Position = vertex1
'            pickedTriangle(1).Position = vertex2
'            pickedTriangle(2).Position = vertex3
'            test = closestIntersection
'        End If
'    End If
'    Return intersection.Value
'End Function


'=================================================================================================================

''Public Sub RayIntersectsTriangle(ByRef ray As Ray, ByRef vertex1 As Vector3, ByRef vertex2 As Vector3, ByRef vertex3 As Vector3, ByRef result As System.Nullable(Of Single))
'Dim edge1 As Vector3, edge2 As Vector3
'        Vector3.Subtract(vertex2, vertex1, edge1)
'        Vector3.Subtract(vertex3, vertex1, edge2)
'Dim directionCrossEdge2 As Vector3
'        Vector3.Cross(ray.Direction, edge2, directionCrossEdge2)
'Dim determinant As Single
'        Vector3.Dot(edge1, directionCrossEdge2, determinant)
'        If (determinant > -Single.Epsilon) And (determinant < Single.Epsilon) Then
'            result = Nothing
'            Return
'        End If
'Dim inverseDeterminant As Single = 1.0F / determinant
'Dim distanceVector As Vector3
'        Vector3.Subtract(ray.Position, vertex1, distanceVector)
'Dim triangleU As Single
'        Vector3.Dot(distanceVector, directionCrossEdge2, triangleU)
'        triangleU = triangleU * inverseDeterminant
'        If triangleU < 0 Or triangleU > 1 Then
'            result = Nothing
'            Return
'        End If
'Dim distanceCrossEdge1 As Vector3
'        Vector3.Cross(distanceVector, edge1, distanceCrossEdge1)
'Dim triangleV As Single
'        Vector3.Dot(ray.Direction, distanceCrossEdge1, triangleV)
'        triangleV = triangleV * inverseDeterminant
'        If triangleV < 0 Or (triangleU + triangleV) > 1 Then
'            result = Nothing
'            Return
'        End If
'Dim rayDistance As Single
'        Vector3.Dot(edge2, distanceCrossEdge1, rayDistance)
'        rayDistance = rayDistance * inverseDeterminant
'        If rayDistance < 0 Then
'            result = Nothing
'            Return
'        End If
'        result = rayDistance
'    End Sub

'Public Function Distance(ByVal Ray As Ray, ByVal Model As Model, ByVal ModelWorldTransforms As Matrix) As String
'    Dim Vertex1 As Vector3, Vertex2 As Vector3, Vertex3 As Vector3
'    Dim ClosestIntersection As System.Nullable(Of Single) = Nothing
'    Dim InverseTransform As Matrix = Matrix.Invert(ModelWorldTransforms)
'    Ray.Position = Vector3.Transform(Ray.Position, InverseTransform)
'    Ray.Direction = Vector3.TransformNormal(Ray.Direction, InverseTransform)
'    Dim tagData As Dictionary(Of String, Object) = DirectCast(Model.Tag, Dictionary(Of String, Object))
'    If tagData Is Nothing Then
'        Throw New InvalidOperationException("Model.Tag is not set correctly. Make sure your model was built using the custom TrianglePickingProcessor.")
'    End If
'    If DirectCast(tagData("BoundingSphere"), BoundingSphere).Intersects(Ray) Is Nothing Then
'        Return Nothing
'    Else
'        Dim Vertices As Vector3() = DirectCast(tagData("Vertices"), Vector3())
'        For i As Integer = 0 To Vertices.Length - 1 Step 3
'            Dim intersection As System.Nullable(Of Single)
'            RayIntersectsTriangle(Ray, Vertices(i), Vertices(i + 1), Vertices(i + 2), intersection)
'            If intersection IsNot Nothing Then
'                If (ClosestIntersection Is Nothing) Or (intersection < ClosestIntersection) Then
'                    ClosestIntersection = intersection.Value
'                    Vector3.Transform(Vertices(i), ModelWorldTransforms, Vertex1)
'                    Vector3.Transform(Vertices(i + 1), ModelWorldTransforms, Vertex2)
'                    Vector3.Transform(Vertices(i + 2), ModelWorldTransforms, Vertex3)
'                End If
'            End If
'        Next
'    End If
'    Return ClosestIntersection.ToString 'If you use .value it returns an error, so we use a string instead for now
'End Function